#include "GPSSatelliteOrbit.h"
#include "Utils.h"
#include <cmath>
#include <stdexcept>
#include <iostream>

GPSSatelliteOrbit::GPSSatelliteOrbit()
    : toe_(0.0), sqrtA_(0.0), e_(0.0), i0_(0.0), OMEGA0_(0.0), omega_(0.0), M0_(0.0),
      deltan_(0.0), idot_(0.0), OMEGA_DOT_(0.0),
      cuc_(0.0), cus_(0.0), crc_(0.0), crs_(0.0), cic_(0.0), cis_(0.0),
      af0_(0.0), af1_(0.0), af2_(0.0) {
    // 初始化成员变量
}

GPSSatelliteOrbit::~GPSSatelliteOrbit() {
}

void GPSSatelliteOrbit::setOrbitParams(const std::vector<double>& params) {
    // params索引需根据实际RINEX文件中导航消息的顺序调整
    if (params.size() < 20) {
        throw std::runtime_error("轨道参数数量不足");
    }
    
    toe_ = params[0];     // 星历参考时间(GPS周内秒)
    sqrtA_ = params[1];   // 轨道长半轴平方根(m^0.5)
    e_ = params[2];       // 轨道偏心率
    i0_ = params[3];      // 轨道倾角(rad)
    OMEGA0_ = params[4];  // 升交点赤经(rad)
    omega_ = params[5];   // 近地点角距(rad)
    M0_ = params[6];      // 平近点角(rad)
    
    deltan_ = params[7];    // 平均角速度改正项(rad/s)
    idot_ = params[8];      // 轨道倾角变化率(rad/s)
    OMEGA_DOT_ = params[9]; // 升交点赤经变化率(rad/s)
    
    cuc_ = params[10];    // 纬度幅角余弦调和改正项(rad)
    cus_ = params[11];    // 纬度幅角正弦调和改正项(rad)
    crc_ = params[12];    // 径向余弦调和改正项(m)
    crs_ = params[13];    // 径向正弦调和改正项(m)
    cic_ = params[14];    // 轨道倾角余弦调和改正项(rad)
    cis_ = params[15];    // 轨道倾角正弦调和改正项(rad)
    
    // 钟差参数（如果可用）
    if (params.size() >= 19) {
        af0_ = params[16];   // 钟差多项式常数项(s)
        af1_ = params[17];   // 钟差多项式一次项(s/s)
        af2_ = params[18];   // 钟差多项式二次项(s/s^2)
    }
}

std::vector<double> GPSSatelliteOrbit::computePosition(double time) const {
    std::vector<double> pos(3, 0.0);
    std::vector<double> vel(3, 0.0);
    
    // 调用同时计算位置和速度的函数
    computePosVel(time, pos, vel, nullptr);
    
    return pos;
}

std::vector<double> GPSSatelliteOrbit::computeVelocity(double time) const {
    std::vector<double> pos(3, 0.0);
    std::vector<double> vel(3, 0.0);
    
    // 调用同时计算位置和速度的函数
    computePosVel(time, pos, vel, nullptr);
    
    return vel;
}

bool GPSSatelliteOrbit::computePosVel(double time, std::vector<double>& pos, std::vector<double>& vel, std::vector<double>* dts) const {
    // 确保输出向量有正确的大小
    pos.resize(3, 0.0);
    vel.resize(3, 0.0);
    if (dts != nullptr) {
        dts->resize(2, 0.0); // 卫星钟差和钟漂
    }
    
    // 如果设置了使用精密星历且SP3插值器有效
    if (usePreciseOrbit_ && sp3Interpolator_ != nullptr) {
        // SP3数据通常包含4个分量: [x,y,z,clk]
        std::vector<double> sp3Pos(4, 0.0);
        std::vector<double> sp3Vel(4, 0.0);
        
        if (sp3Interpolator_->getSatellitePosVel(prn_, time, sp3Pos, &sp3Vel)) {
            // 复制前3个分量为位置
            for (int i = 0; i < 3; i++) {
                pos[i] = sp3Pos[i];
                vel[i] = sp3Vel[i];
            }
            
            // 如果需要输出钟差和钟漂
            if (dts != nullptr) {
                (*dts)[0] = sp3Pos[3];  // 钟差(s)
                (*dts)[1] = sp3Vel[3];  // 钟漂(s/s)
            }
            
            return true;
        }
        
        // 如果精密星历计算失败，回退到广播星历
        std::cout << "警告: 卫星 " << prn_ << " 在时间 " << time 
                  << " 的精密星历计算失败，回退到广播星历" << std::endl;
    }
    
    // 广播星历计算
    
    // 计算卫星钟差
    double dt = computeSatClockBias(time);
    
    // 时间差，考虑卫星钟差修正
    double tk = time - toe_ - dt;
    
    // 处理周跳变
    if (tk > 302400.0) tk -= 604800.0;
    else if (tk < -302400.0) tk += 604800.0;
    
    // 平均角速度
    double n0 = sqrt(GM / pow(sqrtA_, 6));  // sqrt(GM/a^3)
    double n = n0 + deltan_;               // 考虑摄动项
    
    // 平近点角
    double M = M0_ + n * tk;
    
    // 解开普勒方程
    double E = solveKeplerEquation(M, e_);
    double sinE = sin(E);
    double cosE = cos(E);
    
    // 计算真近点角
    double v = atan2(sqrt(1.0 - e_ * e_) * sinE, cosE - e_);
    
    // 升交角距
    double u0 = v + omega_;
    
    // 计算摄动项
    double sin2u = sin(2.0 * u0);
    double cos2u = cos(2.0 * u0);
    
    double du = cus_ * sin2u + cuc_ * cos2u;
    double dr = crs_ * sin2u + crc_ * cos2u;
    double di = cis_ * sin2u + cic_ * cos2u;
    
    // 摄动后的参数
    double u = u0 + du;
    double r = sqrtA_ * sqrtA_ * (1.0 - e_ * cosE) + dr;
    double i = i0_ + di + idot_ * tk;
    
    // 卫星在轨道平面内的位置
    double x_orb = r * cos(u);
    double y_orb = r * sin(u);
    
    // 升交点赤经，考虑地球自转
    double O = OMEGA0_ + (OMEGA_DOT_ - OMEGA_E) * tk - OMEGA_E * toe_;
    
    // 三角函数值
    double sinO = sin(O);
    double cosO = cos(O);
    double sini = sin(i);
    double cosi = cos(i);
    
    // 计算ECEF坐标 - 位置
    pos[0] = x_orb * cosO - y_orb * cosi * sinO;
    pos[1] = x_orb * sinO + y_orb * cosi * cosO;
    pos[2] = y_orb * sini;
    
    // 计算速度需要的额外参数
    double Edot = n / (1.0 - e_ * cosE);                   // dE/dt
    double vdot = Edot * sqrt(1.0 - e_ * e_) / (1.0 - e_ * cosE);  // dv/dt
    double udot = vdot + 2.0 * (cus_ * cos2u - cuc_ * sin2u) * vdot;  // du/dt
    double rdot = sqrtA_ * sqrtA_ * e_ * sinE * n / (1.0 - e_ * cosE) +
                  2.0 * (crs_ * cos2u - crc_ * sin2u) * vdot;      // dr/dt
    double idot = idot_ + (cis_ * cos2u - cic_ * sin2u) * 2.0 * vdot;  // di/dt
    double Odot = OMEGA_DOT_ - OMEGA_E;                   // dO/dt
    
    // 卫星在轨道平面内的速度
    double xdot_orb = rdot * cos(u) - r * sin(u) * udot;
    double ydot_orb = rdot * sin(u) + r * cos(u) * udot;
    
    // 计算ECEF坐标 - 速度，考虑地球自转
    vel[0] = -Odot * pos[1] + xdot_orb * cosO - ydot_orb * cosi * sinO + y_orb * sini * sinO * idot;
    vel[1] =  Odot * pos[0] + xdot_orb * sinO + ydot_orb * cosi * cosO - y_orb * sini * cosO * idot;
    vel[2] =  ydot_orb * sini + y_orb * cosi * idot;
    
    // 如果需要输出钟差和钟漂
    if (dts != nullptr) {
        // 钟差
        (*dts)[0] = af0_ + af1_ * tk + af2_ * tk * tk;
        
        // 相对论效应修正
        (*dts)[0] -= 2.0 * sqrt(GM) * e_ * sinE / (C_LIGHT * C_LIGHT);
        
        // 钟漂
        (*dts)[1] = af1_ + 2.0 * af2_ * tk;
        
        // 相对论效应修正的变化率
        (*dts)[1] -= 2.0 * sqrt(GM) * e_ * cosE * Edot / (C_LIGHT * C_LIGHT);
    }
    
    return true;
}

double GPSSatelliteOrbit::computeSatClockBias(double time) const {
    // 时间差
    double dt = time - toe_;
    
    // 处理周跳变
    if (dt > 302400.0) dt -= 604800.0;
    else if (dt < -302400.0) dt += 604800.0;
    
    // 钟差多项式计算
    double dts = af0_ + af1_ * dt + af2_ * dt * dt;
    
    // 相对论效应修正
    // 平近点角
    double tk = time - toe_;
    if (tk > 302400.0) tk -= 604800.0;
    else if (tk < -302400.0) tk += 604800.0;
    
    double n0 = sqrt(GM / pow(sqrtA_, 6));
    double n = n0 + deltan_;
    double M = M0_ + n * tk;
    double E = solveKeplerEquation(M, e_);
    
    // 相对论效应: -2(r·v)/c^2
    double rel_effect = -2.0 * sqrt(GM) * e_ * sin(E) / (C_LIGHT * C_LIGHT);
    
    return dts + rel_effect;
}

double GPSSatelliteOrbit::solveKeplerEquation(double M, double e) const {
    const int MAX_ITER = 30;
    const double EPSILON = 1e-12;
    
    double E = M;  // 初始值
    
    for (int i = 0; i < MAX_ITER; i++) {
        double E_new = M + e * sin(E);
        if (fabs(E_new - E) < EPSILON) {
            return E_new;
        }
        E = E_new;
    }
    
    // 若迭代未收敛，返回最后结果
    return E;
} 